#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <vector>
#include <string>
#include <algorithm>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

using namespace std;
using namespace cv;

static void stereoCalib(const vector<string>& imagelist,Size boardSize,bool useCalibrated=true,bool showRectified=true)
{
    if(imagelist.size()%2!=0)
    {
        cout<<"Error:the image list contains odd (non-even) number of elements\n";   //双目相机照片对应
        return ;
    }
    
    bool displayCorners=true;
    const int maxScale =2;
    const float squareSize =1.f;  //设置实际大小
    
    vector<vector<Point2f>> imagePoint[2];
    vector<vector<Point3f>> objectPoints;
    Size imageSize;
    
    int i,j,k,nimages=(int)imagelist.size()/2;
    
    imagePoint[0].resize(nimages);
    imagePoint[1].resize(nimages);
    vector<string>goodImageList;
    
    for(i=j=0;i<nimages;++i)
    {
        for(k=0;k<2;k++)
        {
            const string& filename=imagelist[i*2+k];
            Mat image=imread(filename,IMREAD_COLOR);
            if(image.empty())
            {
                cout<<"the image"<<filename<<"is erroe"<<endl;
                break;
            }
            if(imageSize==Size())  //Size???
            {
                imageSize=image.size();
            }
            else if(image.size()!=imageSize)
            {
                cout<<"the image "<<filename<<"has the size different from the first iamge size"<<endl;
                break;
            }
            
            bool found=false;
            vector<Point2f>& corners=imagePoint[k][j];
            for(int scale =1;scale<=maxScale;scale++)
            {
                Mat timg;
                if(scale==1)
                    timg=image;
                else
                    resize(image,timg,Size(),scale,scale); //调整图像大小；
                
                //发现棋盘格内部的角点；
                found=findChessboardCorners(timg,boardSize,corners,CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);
                
                if(found)
                    {
                        if(scale>1)
                        {
                            Mat cornersMat(corners);
                            cornersMat*=1./scale;
                        }
                        break;
                    }
            }
            
            if(displayCorners)
            {
                cout<<filename<<endl;
                Mat cimg,cimg1;
                cvtColor(image,cimg,COLOR_GRAY2BGR);
                
                drawChessboardCorners(cimg,boardSize,corners,found);
                double sf=640./MAX(image.rows,image.cols);
                resize(cimg,cimg1,Size(),sf,sf);
                imshow("corners",cimg1);
                char c=(char)waitKey(500);
                if(c==27||c=='q'||c=='Q')
                    exit(-1);
            }
            else
            {
                putchar('.');
            }
            
            if(!found)
            {
                break;
            }
            
            cornerSubPix(image,corners,Size(11,11),Size(-1,-1),
                         TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,30,0.01)); //亚像素角点检测；
        }
        
        if(k==2)
        {
            goodImageList.push_back(imagelist[i*2]);
            goodImageList.push_back(imagelist[i*2+1]);
            j++;
        }
    }
    cout<<j<<"pairs have been successfully detected.\n";
    nimages=j;
    if(nimages<2)
    {
        cout<<"Error: too little pairs to run the calibration\n";
        return ;
    }
    
    imagePoint[0].resize(nimages);
    imagePoint[1].resize(nimages);
    objectPoints.resize(nimages);
    
    for(i=0;i<nimages;++i)
    {
        for(j=0;j<boardSize.height;++j)
        {
            for(k=0;k<boardSize.width;++k)
            {
                objectPoints[i].push_back(Point3f(k*squareSize,j*squareSize,0));
            }
        }
    }
    
    cout<<"running stereo calibration ...\n";
    
    Mat cameraMatrix[2],distcoeffs[2];
    cameraMatrix[0]=Mat::eye(3,3,CV_64F);
    cameraMatrix[1]=Mat::eye(3,3,CV_64F);
    Mat R,T,E,F,preViewError;
     
    double rms=stereoCalibrate(objectPoints,imagePoint[0],imagePoint[1],cameraMatrix[0],distcoeffs[0],cameraMatrix[0],
                               distcoeffs[1],imageSize,R,T,E,F,CALIB_FIX_INTRINSIC,
                               TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,100,1e-5));
    /**
    stereoCalibrate() 是用来标定一个立体摄像头的，也就是同时标定两个摄像头。标定的结果除了能够求出两个摄像头的内外参数矩阵，跟能够得出两个摄像头位置关系R,T。R-输出型，第一和第二个摄像机之间的旋转矩阵
    T-输出型，第一和第二个摄像机之间的平移矩阵
    E-输出型，本质矩阵，其物理意义是左右图像坐标系相互转换的矩阵，描述的是同一空间点投影在左右摄像机图像平面上对应点之间的关系，单位为mm。
    F-输出型，基础矩阵，同一物理点在左右摄像机图像平面上投影在像素坐标系下的关系：
    */
    cout<<"done with RMS error="<<rms<<endl;
    
    double err=0;
    int npoints=0;
    vector<Vec3f> line[2];
    for(int i=0;i<nimages;++i)
    {
        int npt=(int)imagePoint[0][i].Size();
        Mat imgpt[2];
        for(k=0;k<2;++k)
        {
            imgpt[k]=Mat(imagePoint[k][i]);
            undistortPoints(imgpt[k],imgpt[k],cameraMatrix[k],distcoeffs[k],Mat(),cameraMatrix());
            computeCorrespondEpilines(imgpt[k],k+1,F,lines[k]);
        }
        for(j=0;j<npt;j++)
        {
            double errij=fabs(imagePoint[0][i][j].x*lines[1][j][0]+imagePoint[0][i][j].y*line[1][j][1]
                               +lines[1][j][2])+fabs(imagePoint[1][i][j].x*lines[0][j][0]
                               +imagePoint[1][i][j].y*lines[0][j][1]+lines[0][j][2]);
            err+=errij;
        }
        npoints+=npt;
    }
    cout<<"average reprojection err = "<<err/npoints<<endl;
    
    //save intrinsics parameters
    FileStorage fs("intrinsics.yml",CV_STORAGE_WRITE);
    if(fs.isOpened())
    {
        fs<<"M1"<<cameraMatrix[0]<<"D1"<<distcoeffs[0]<<
            "M2"<<cameraMatrix[1]<<"D2"<<distcoeffs[1];
        fs.release();
    }
    else
        cout<<"Error: can not save the intrinsics parameter\n";
    
    Mat R1,R2,P1,P2,Q;
    Rect validRoi[2];
    
    stereoRectify(cameraMatrix[0],distcoeffs[0],cameraMatrix[1],distcoeffs[1],
                  iamgeSize,R,T,R1,R2,P1,P2,Q,CALIB_ZERO_DISPARITY,1,imageSize,&vailRoi[0],&vailRoi[1]);
    fs.open("extrinsics.yml",CV_STORAGE_WRITE);
    if(fs.isOpened())
    {
        fs<<"R "<,R<<"T "<<T<<"R1 "<<R1<<"R2 "<<R2<<"P1 "<<P1<<"P2 "<<P2<<"Q "<<Q;
        fs.release();
    }
    else
    {
        cout<<"Error:can not save the extrinsic parameters\n";
    }
    
    //OpenCV can handle left-right
    //or up-down camera arrangements
    bool isVerticalStereo=fabs(P2.at<double>(1,3))>fabs(P2.at<double>(0,3));
    
    //compute and display RECTIFICATION
    if(!showRectified)
    {
        return;
    }
    
    Mat rmap[2][2];
    //IF BY CALIBRATED(BOUGUET'S METHOD)
    if(useCalibrated)
    {
        //compute everting;
    }
    //OR else hartley's method
    
    else   //use intrinsic parameter of each camera, but computer the rectification transfoemation directly from 
           //the fundamental matrix
    {
        vector<Point2f> allimgpt[2];
        for(k=0;k<2;++k)
        {
            for(i=0;i<nimages;i++)
            {
                std::copy (imagePoint[k][i].begin(),iamgePoint[k][i].end,back_inserter(allimgpt[k]));
            }
            F=findFundamentalMat(Mat(allimgpt[0]),Mat(allimgpt[1]),FM_8POINT,0,0);
            Mat H1,H2;
            stereoRectifyUncalibrated(Mat(allimgpt[0]))
    
    
    
    
    
    
    
    
    
    
    
        
        
        
    
    






int main(int argc, char **argv) 
{
    std::cout << "Hello, world!" << std::endl;
    return 0;
}
